#include <unitree_cameras.h>

using namespace cv;

int yellow_lower[3] = {15, 120, 120};
int yellow_upper[3] = {45, 240, 200};

int blue_lower[3] = {110,  80,  90};
int blue_upper[3] = {180, 220, 210};

cv::Rect box;


bool get_color_block(cv::Mat img, int color_lower[], int color_upper[], int square=0)
{
    Mat dst;
	Mat kernel;
	kernel = getStructuringElement(MORPH_RECT, Size(5, 5));
 
	std::vector<std::vector<Point>> contours;
	std::vector<Vec4i> hireachy;
	Point2f center;
    float radius = 20;

    Mat hsv;
	cvtColor(img, hsv, COLOR_BGR2HSV);
	inRange(hsv, 
            Scalar(color_lower[0], color_lower[1], color_lower[2]), 
            Scalar(color_upper[0], color_upper[1], color_upper[2]), 
            dst);
	//开操作
	morphologyEx(dst, dst, MORPH_OPEN, kernel);
	morphologyEx(dst, dst, MORPH_OPEN, kernel);
	morphologyEx(dst, dst, MORPH_OPEN, kernel);
	morphologyEx(dst, dst, MORPH_OPEN, kernel);
	//获取边界
	findContours(dst, contours, hireachy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE, Point(0, 0));
	//框选面积最大的边界
	if (contours.size() > 0)
	{
		double maxArea = 0;
		for (int i = 0; i < contours.size(); i++)
		{
			double area = contourArea(contours[static_cast<int>(i)]);
			if (area > maxArea)
			{
				maxArea = area;
				box = boundingRect(contours[static_cast<int>(i)]);
				minEnclosingCircle(contours[static_cast<int>(i)], center, radius);
			}
		}
        if(!square) return true;
        if(square && maxArea >= square) return true;
        else return false;
	}
    else return false;
}

void processing(cv::Mat frame)
{
    if(get_color_block(frame, blue_lower, blue_upper, 15000))
    {
        rectangle(frame, box, Scalar(0,255,255), 2);
    }
    if(get_color_block(frame, yellow_lower, yellow_upper))
    {
        rectangle(frame, box, Scalar(0,255,255), 2);
    }
}

int main()
{
    Cameras camera(0);
    camera.show(processing);
    camera.stop();
    return 0;
}